#include "vehicle.h"
#include "misc/C2DMatrix.h"
#include "algorithms/flocking.h"

using std::vector;
using std::list;

Vehicle::Vehicle(	InGame* pWorld,
					double		rotation,
					Vector2D	velocity,
					double		max_force,
					double		max_speed,
					double		max_turn_rate,
					double		mass			):	MovingEntity(	Vector2D(sin(rotation),-cos(rotation)),
																	velocity,
																	max_speed,
																	max_turn_rate,
																	max_force,
																	mass),
													m_vSmoothedHeading(Vector2D(0,0)),
													m_bSmoothingOn(true),
													m_pWorld(pWorld)
{
	//set up the flocking class
	m_pFlocking = new Flocking(this);    

	//set up the smoother
	m_pHeadingSmoother = new Smoother<Vector2D>(10.0, Vector2D(0.0, 0.0));
 
}

/*-------------------------------------------------------------------------------------------------------------*/
Vehicle::~Vehicle()
{
	delete m_pFlocking;
	delete m_pHeadingSmoother;
}

/*-------------------------------------------------------------------------------------------------------------*/
bool Vehicle::Initialize()
{
	if (!this->LoadIni("data/graphics/vehicle/butterfly.spr"))
	{
		return false;
	}

	return true;
}

/*-------------------------------------------------------------------------------------------------------------*/
void Vehicle::Update(float dt)
{    
	//keep a record of its old position so we can update its cell later
	//in this method
	Vector2D OldPos = Vector2D(GetPosX(), GetPosY());

	Vector2D SteeringForce;

	//calculate the combined force from each steering behavior in the 
	//vehicle's list
	SteeringForce = m_pFlocking->Calculate();

	//Acceleration = Force/Mass
	Vector2D acceleration = SteeringForce / m_dMass;

	//update velocity
	m_vVelocity += acceleration * dt; 

	//make sure vehicle does not exceed maximum velocity
	m_vVelocity.Truncate(m_dMaxSpeed);

	//update the position
	Vector2D vPos = Vector2D(GetPosX(), GetPosY());
	
	vPos += m_vVelocity * dt;

	//update the heading if the vehicle has a non zero velocity
	if (m_vVelocity.LengthSq() > 0.00000001)
	{    
		m_vHeading = Vec2DNormalize(m_vVelocity);

		m_vSide = m_vHeading.Perp();
	}

	//treat the screen as a toroid
	WrapAround(vPos, 400.0, 300.0);

	SetPos(vPos.x, vPos.y);

	if (isSmoothingOn())
	{
		m_vSmoothedHeading = m_pHeadingSmoother->Update(Heading());
	}
}


/*-------------------------------------------------------------------------------------------------------------*/
void Vehicle::Draw()
{ 
	Sprite::Draw();  
}
